/*
 * servos_motor.c
 *
 *  Created on: Apr 23, 2013
 *      Author: Daniel
 */

#include "DSP28x_Project.h"
#include "servos_motor.h"

void InitPWM() {
	InitPWM2();
	InitPWM3();
	InitPWM4();
	InitPWM5();
	return;
}

void InitPWM2() {
	EALLOW;
	GpioCtrlRegs.GPAPUD.bit.GPIO2 = 1;	// disable pull-up on GPIO2
	GpioCtrlRegs.GPAMUX1.bit.GPIO2 = 1; // Configure GPIO2 as EPWM2A
	GpioCtrlRegs.GPAPUD.bit.GPIO3 = 1;	// disable pull-up on GPIO3
	GpioCtrlRegs.GPAMUX1.bit.GPIO3 = 1;	// Configure GPIO3 as EPWM2B

	EPwm2Regs.TBCTL.bit.CTRMODE = TB_COUNT_UP;	// counter counts from 0 to EPWM2345_TBPRD and then returns to 0
	EPwm2Regs.TBPRD = EPWM2345_TBPRD;			// set timer period
	EPwm2Regs.TBCTL.bit.PHSEN = TB_ENABLE;		// disable phase loading
	EPwm2Regs.TBPHS.half.TBPHS = 0x0000;		// phase is 0
	EPwm2Regs.TBCTR = 0x0000;					// clear counter
	EPwm2Regs.TBCTL.bit.HSPCLKDIV = 0x5;	// divide the clock input by 10
	EPwm2Regs.TBCTL.bit.CLKDIV = 0x1;		// divide the clock input by 2

	// setup shadow register load on ZERO
	EPwm2Regs.CMPCTL.bit.SHDWAMODE = CC_SHADOW;
	EPwm2Regs.CMPCTL.bit.LOADAMODE = CC_CTR_ZERO;
	EPwm2Regs.CMPCTL.bit.SHDWBMODE = CC_SHADOW;
	EPwm2Regs.CMPCTL.bit.LOADAMODE = CC_CTR_ZERO;

	// set compare value
	EPwm2Regs.CMPA.half.CMPA = EPWM2_CMPA_Default;
	EPwm2Regs.CMPB = EPWM2_CMPB_Off;

	// set actions
	EPwm2Regs.AQCTLA.bit.ZRO = AQ_SET;		// set PWM2A on counter = zero
	EPwm2Regs.AQCTLA.bit.CAU = AQ_CLEAR;	// clear PWM2A on counter = EPWM2_CMPA
	EPwm2Regs.AQCTLB.bit.ZRO = AQ_SET;		// set PWM2B on counter = zero
	EPwm2Regs.AQCTLB.bit.CBU = AQ_CLEAR;	// clear PWM2B on counter = EPWM2_CMPB

	// disable interrupts
	EPwm2Regs.ETSEL.bit.INTEN = 0;
}

void InitPWM3() {
	EALLOW;
	GpioCtrlRegs.GPAPUD.bit.GPIO4 = 1;	// disable pull-up on GPIO4
	GpioCtrlRegs.GPAMUX1.bit.GPIO4 = 1;	// Configure GPIO4 as EPWM3A
	GpioCtrlRegs.GPAPUD.bit.GPIO5 = 1;	// disable pull-up on GPIO5
	GpioCtrlRegs.GPAMUX1.bit.GPIO5 = 1;	// Configure GPIO5 as EPWM3B

	EPwm3Regs.TBCTL.bit.CTRMODE = TB_COUNT_UP;	// counter counts from 0 to EPWM2345_TBPRD and then returns to 0
	EPwm3Regs.TBPRD = EPWM2345_TBPRD;			// set timer period
	EPwm3Regs.TBCTL.bit.PHSEN = TB_ENABLE;		// disable phase loading
	EPwm3Regs.TBPHS.half.TBPHS = 0x0000;		// phase is 0
	EPwm3Regs.TBCTR = 0x0000;					// clear counter
	EPwm3Regs.TBCTL.bit.HSPCLKDIV = 0x5;	// divide the clock input by 10
	EPwm3Regs.TBCTL.bit.CLKDIV = 0x1;		// divide the clock input by 2

	// setup shadow register load on ZERO
	EPwm3Regs.CMPCTL.bit.SHDWAMODE = CC_SHADOW;
	EPwm3Regs.CMPCTL.bit.LOADAMODE = CC_CTR_ZERO;
	EPwm3Regs.CMPCTL.bit.SHDWBMODE = CC_SHADOW;
	EPwm3Regs.CMPCTL.bit.LOADAMODE = CC_CTR_ZERO;

	// set compare value
	EPwm3Regs.CMPA.half.CMPA = EPWM3_CMPA_Default;
	EPwm3Regs.CMPB = EPWM3_CMPB_Default;

	// set actions
	EPwm3Regs.AQCTLA.bit.ZRO = AQ_SET;		// set PWM3A on counter = zero
	EPwm3Regs.AQCTLA.bit.CAU = AQ_CLEAR;	// clear PWM3A on counter = EPWM3_CMPA
	EPwm3Regs.AQCTLB.bit.ZRO = AQ_SET;		// set PWM3B on counter = zero
	EPwm3Regs.AQCTLB.bit.CBU = AQ_CLEAR;	// clear PWM3B on counter = EPWM3_CMPB

	// disable interrupts
	EPwm3Regs.ETSEL.bit.INTEN = 0;
}

void InitPWM4() {
	EALLOW;
	GpioCtrlRegs.GPAPUD.bit.GPIO6 = 1;	// disable pull-up on GPIO6
	GpioCtrlRegs.GPAMUX1.bit.GPIO6 = 1;	// Configure GPIO0 as EPWM4A

	EPwm4Regs.TBCTL.bit.CTRMODE = TB_COUNT_UP;	// counter counts from 0 to EPWM2345_TBPRD and then returns to 0
	EPwm4Regs.TBPRD = EPWM2345_TBPRD;			// set timer period
	EPwm4Regs.TBCTL.bit.PHSEN = TB_DISABLE;		// disable phase loading
	EPwm4Regs.TBPHS.half.TBPHS = 0x0000;		// phase is 0
	EPwm4Regs.TBCTR = 0x0000;					// clear counter
	EPwm4Regs.TBCTL.bit.HSPCLKDIV = 0x5;	// divide the clock input by 10
	EPwm4Regs.TBCTL.bit.CLKDIV = 0x1;		// divide the clock input by 2

	// setup shadow register load on ZERO
	EPwm4Regs.CMPCTL.bit.SHDWAMODE = CC_SHADOW;
	EPwm4Regs.CMPCTL.bit.LOADAMODE = CC_CTR_ZERO;

	// set compare value
	EPwm4Regs.CMPA.half.CMPA = EPWM4_CMPA_Default;

	// set actions
	EPwm4Regs.AQCTLA.bit.ZRO = AQ_SET;		// set PWM3A on counter = zero
	EPwm4Regs.AQCTLA.bit.CAU = AQ_CLEAR;	// clear PWM3A on counter = EPWM3_CMPA

	// disable interrupts
	EPwm4Regs.ETSEL.bit.INTEN = 0;
}

void InitPWM5() {
	EALLOW;
	GpioCtrlRegs.GPAPUD.bit.GPIO8 = 1;	// disable pull-up on GPIO8
	GpioCtrlRegs.GPAMUX1.bit.GPIO8 = 1;	// Configure GPIO0 as EPWM5A

	EPwm5Regs.TBCTL.bit.CTRMODE = TB_COUNT_UP;	// counter counts from 0 to EPWM2345_TBPRD and then returns to 0
	EPwm5Regs.TBPRD = EPWM2345_TBPRD;			// set timer period
	EPwm5Regs.TBCTL.bit.PHSEN = TB_DISABLE;		// disable phase loading
	EPwm5Regs.TBPHS.half.TBPHS = 0x0000;		// phase is 0
	EPwm5Regs.TBCTR = 0x0000;					// clear counter
	EPwm5Regs.TBCTL.bit.HSPCLKDIV = 0x5;	// divide the clock input by 10
	EPwm5Regs.TBCTL.bit.CLKDIV = 0x1;		// divide the clock input by 2

	// setup shadow register load on ZERO
	EPwm5Regs.CMPCTL.bit.SHDWAMODE = CC_SHADOW;
	EPwm5Regs.CMPCTL.bit.LOADAMODE = CC_CTR_ZERO;

	// set compare value
	EPwm5Regs.CMPA.half.CMPA = EPWM5_CMPA_Default;

	// set actions
	EPwm5Regs.AQCTLA.bit.ZRO = AQ_SET;		// set PWM5A on counter = zero
	EPwm5Regs.AQCTLA.bit.CAU = AQ_CLEAR;	// clear PWM5A on counter = EPWM5_CMPA

	// disable interrupts
	EPwm5Regs.ETSEL.bit.INTEN = 0;
}

void changePWM2A(int new_EPWM2_CMPA) {
	if(new_EPWM2_CMPA > EPWM2_CMPA_Max) {
		EPwm2Regs.CMPA.half.CMPA = EPWM2_CMPA_Max;
	} else if(new_EPWM2_CMPA < EPWM2_CMPA_Min) {
		EPwm2Regs.CMPA.half.CMPA = EPWM2_CMPA_Min;
	} else {
		EPwm2Regs.CMPA.half.CMPA = new_EPWM2_CMPA;
	}
}

void changePWM3A(int new_EPWM3_CMPA) {
	if(new_EPWM3_CMPA > EPWM3_CMPA_Max) {
		EPwm3Regs.CMPA.half.CMPA = EPWM3_CMPA_Max;
	} else if(new_EPWM3_CMPA < EPWM3_CMPA_Min) {
		EPwm3Regs.CMPA.half.CMPA = EPWM3_CMPA_Min;
	} else {
		EPwm3Regs.CMPA.half.CMPA = new_EPWM3_CMPA;
	}
}

void changePWM3B(int new_EPWM3_CMPB) {
	if(new_EPWM3_CMPB > EPWM3_CMPB_Max) {
		EPwm3Regs.CMPB = EPWM3_CMPB_Max;
	} else if(new_EPWM3_CMPB < EPWM3_CMPB_Min) {
		EPwm3Regs.CMPB = EPWM3_CMPB_Min;
	} else {
		EPwm3Regs.CMPB = new_EPWM3_CMPB;
	}
}

void changePWM4A(int new_EPWM4_CMPA) {
	if(new_EPWM4_CMPA > EPWM4_CMPA_Max) {
		EPwm4Regs.CMPA.half.CMPA = EPWM4_CMPA_Max;
	} else if(new_EPWM4_CMPA < EPWM4_CMPA_Min) {
		EPwm4Regs.CMPA.half.CMPA = EPWM4_CMPA_Min;
	} else {
		EPwm4Regs.CMPA.half.CMPA = new_EPWM4_CMPA;
	}
}

void changePWM5A(int new_EPWM5_CMPA) {
	if(new_EPWM5_CMPA > EPWM5_CMPA_Max) {
		EPwm5Regs.CMPA.half.CMPA = EPWM5_CMPA_Max;
	} else if(new_EPWM5_CMPA < EPWM5_CMPA_Min) {
		EPwm5Regs.CMPA.half.CMPA = EPWM5_CMPA_Min;
	} else {
		EPwm5Regs.CMPA.half.CMPA = new_EPWM5_CMPA;
	}
}
